


/*
FK_Full =
 
 cos(t1)*(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4))
 sin(t1)*(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4))
           l1 + l4*sin(t2 + t3) + l3*sin(t2) + lt*sin(t2 + t3 + t4)
                                                     - t2 - t3 - t4
*/
void FK_Full(const RobotModel& rm, cvec4d* epos, jvec4d* jpos) {

	epos->x = cos(jpos->t1)*(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4));

	epos->y = sin(jpos->t1)*(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4));

	epos->z =  rm->length[0] + rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[2]*sin(jpos->t2) + rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4);

	epos->phi = - jpos->t2 - jpos->t3 - jpos->t4;

}





/*
	V = J*qdot
	[x_dot, y_dot, z_dot, phi_dot] = J*[theta_dot]


J =
 
[ -sin(t1)*(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4)), -cos(t1)*(l4*sin(t2 + t3) + l3*sin(t2) + lt*sin(t2 + t3 + t4)), -cos(t1)*(l4*sin(t2 + t3) + lt*sin(t2 + t3 + t4)), -lt*sin(t2 + t3 + t4)*cos(t1)]
[  cos(t1)*(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4)), -sin(t1)*(l4*sin(t2 + t3) + l3*sin(t2) + lt*sin(t2 + t3 + t4)), -sin(t1)*(l4*sin(t2 + t3) + lt*sin(t2 + t3 + t4)), -lt*sin(t2 + t3 + t4)*sin(t1)]
[                                                                   0,            l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4),            l4*cos(t2 + t3) + lt*cos(t2 + t3 + t4),          lt*cos(t2 + t3 + t4)]
[   


*/
void jacobian(const RobotModel& rm, cvec4d* evel, jvec4d* jpos, jvec4d* jvel) {
	
	evel->x = -sin(jpos->t1)*(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4)) *jvel->t1;\
	      -cos(jpos->t1)*(rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[2]*sin(jpos->t2) + rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4)) *jvel->t2;\
	      -cos(jpos->t1)*(rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4)) *jvel->t3\
	      -rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4)*cos(jpos->t1) *jvel->t4;


	evel->y =  cos(jpos->t1)*(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4)) *jvel->t1;\
	       -sin(jpos->t1)*(rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[2]*sin(jpos->t2) + rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4))  *jvel->t2;\
	       -sin(jpos->t1)*(rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4)) *jvel->t3\
	       -rm->length[4]*sin(jpos->t2 + jpos->t3 + jpos->t4)*sin(jpos->t1) *jvel->t4;


	evel->z = 0*jvel->t1;\
	      +rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4) *jvel->t2;\
	      +rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4) *jvel->t3\ 
	      +rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4) *jvel->t4;


	evel->phi = -jvel->t2; - jvel->t3 - jvel->t4;

}

//void twodof_kine::jacobian( t[], tdot[]) {}


/* 
	qdot = J^-1 * V

	[thetadot] = J^-1 * [xdot, ydot, zdot, phidot]

J_inv =
 
[ -sin(t1)/(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4)), cos(t1)/(l2 + l4*cos(t2 + t3) + l3*cos(t2) + lt*cos(t2 + t3 + t4)),                                               0,                                                   0]
[                                 (cos(t2 + t3)*cos(t1))/(l3*sin(t3)),                                (cos(t2 + t3)*sin(t1))/(l3*sin(t3)),                       sin(t2 + t3)/(l3*sin(t3)),                          -(lt*sin(t4))/(l3*sin(t3))]
[           -(cos(t1)*(l4*cos(t2 + t3) + l3*cos(t2)))/(l3*l4*sin(t3)),          -(sin(t1)*(l4*cos(t2 + t3) + l3*cos(t2)))/(l3*l4*sin(t3)), -(l4*sin(t2 + t3) + l3*sin(t2))/(l3*l4*sin(t3)), (lt*(l3*sin(t3 + t4) + l4*sin(t4)))/(l3*l4*sin(t3))]
[                                      (cos(t1)*cos(t2))/(l4*sin(t3)),                                     (cos(t2)*sin(t1))/(l4*sin(t3)),                            sin(t2)/(l4*sin(t3)),                - (lt*sin(t3 + t4))/(l4*sin(t3)) - 1]
*/
void invjacobian(const RobotModel& rm, cvec4d* evel, jvec4d* jvel) {

	jvel->t1; = -sin(jpos->t1)/(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4)) * evel->x\
	          +cos(jpos->t1)/(rm->length[1] + rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2) + rm->length[4]*cos(jpos->t2 + jpos->t3 + jpos->t4)) * evel->y\
	          +0 * evel->z\
	          +0 * evel->phi;


	jvel->t2; =  (cos(jpos->t2 + jpos->t3)*cos(jpos->t1))/(rm->length[2]*sin(jpos->t3)) * evel->x\
	           +(cos(jpos->t2 + jpos->t3)*sin(jpos->t1))/(rm->length[2]*sin(jpos->t3)) * evel->y\
	           +sin(jpos->t2 + jpos->t3)/(rm->length[2]*sin(jpos->t3)) * evel->z\ 
	           -(rm->length[4]*sin(jpos->t4))/(rm->length[2]*sin(jpos->t3)) * evel->phi;


	jvel->t3 = -(cos(jpos->t1)*(rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2)))/(rm->length[2]*rm->length[3]*sin(jpos->t3)) * evel->x\ 
			  -(sin(jpos->t1)*(rm->length[3]*cos(jpos->t2 + jpos->t3) + rm->length[2]*cos(jpos->t2)))/(rm->length[2]*rm->length[3]*sin(jpos->t3)) * evel->y\
			  -(rm->length[3]*sin(jpos->t2 + jpos->t3) + rm->length[2]*sin(jpos->t2))/(rm->length[2]*rm->length[3]*sin(jpos->t3)) * evel->z\
			  (rm->length[4]*(rm->length[2]*sin(jpos->t3 + jpos->t4) + rm->length[3]*sin(jpos->t4)))/(rm->length[2]*rm->length[3]*sin(jpos->t3)) * evel->phi;



	jvel->t4 = (cos(jpos->t1)*cos(jpos->t2))/(rm->length[3]*sin(jpos->t3)) * evel->x\
			  +(cos(jpos->t2)*sin(jpos->t1))/(rm->length[3]*sin(jpos->t3)) * evel->y\ 
			  +sin(jpos->t2)/(rm->length[3]*sin(jpos->t3)) * evel->x\
			  +(-(rm->length[4]*sin(jpos->t3 + jpos->t4))/(rm->length[3]*sin(jpos->t3)) - 1) * evel->phi;  
}
